u_lim = 10;
u = bfu(size(bfu)*[0;1]).data;
J = f_dgdu(u);

epsilon1 = 10^-3;

token2 = zeros(m*N,1);
token1 = zeros(m*N,1);

% 0: u_i = 0
% 1: u_i \neq 0, |u_i| \neq u_lim (tilde)
% 2: u_i \neq 0, |u_i| = u_lim (bar)

for i=1:m*N
    if abs(abs(u(i))-u_lim) <= epsilon1
        token2(i) = 1;
    elseif abs(u(i)) >= epsilon1
        token1(i) = 1;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% step1 tilde %%%%%%%%%%%%%%%%%%%%%%%%%%

dfdx_til = zeros(sum(token1),1);
dgdx_til = zeros(n,sum(token1));
% dhdx_til = zeros(sum(token1),sum(token1));

count = 0;
for i=1:m*N
    if token1(i) == 1
        count = count + 1;
        dfdx_til(count) = sign(u(i));
        dgdx_til(:,count) = J(:,i);
        % dhdx_til(count,count) = sign(u(i));
    end
end

lambda = dgdx_til' \ -dfdx_til; % uses pseudoinverse if not square
% attempts to minimize the reduced residual
% mu_til = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% step2 bar %%%%%%%%%%%%%%%%%%%%%%%%%%

dfdx_bar = zeros(sum(token2),1);
dgdx_bar = zeros(n,sum(token2));
mu_bar = zeros(sum(token2),1);

count = 0;
for i=1:m*N
    if token2(i) == 1
        count = count + 1;
        dfdx_bar(count) = sign(u(i));
        dgdx_bar(:,count) = J(:,i);
        mu_bar(count) = -(dfdx_bar(count)+dgdx_bar(:,count)'*lambda)/sign(u(i));
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% step3 full %%%%%%%%%%%%%%%%%%%%%%%%%%

% calculates full dfdx
dfdx = zeros(m*N,1);
mu = zeros(m*N,1);
dhdx = zeros(m*N,m*N);

count = 0;
countt = 0;

for i=1:m*N
    if token1(i) == 1
        count = count + 1;
        dfdx(i) = dfdx_til(count);
        dhdx(i,i) = sign(u(i));
        mu(i) = 0;
    elseif token2(i) == 1
        countt = countt + 1;
        dfdx(i) = dfdx_bar(countt);
        dhdx(i,i) = sign(u(i));
        mu(i) = mu_bar(countt);
    else
        dfdx(i) = -J(:,i)'*lambda;
        dhdx(i,i) = 0;
        mu(i) = 0;
    end
end

residual = dfdx + J'*lambda + dhdx'*mu; % calculates the full residual

residual_l2norm = norm(residual,2);